#include "bits/stdc++.h"
#include "ros/ros.h"
#include "unistd.h"
#include <sensor_msgs/LaserScan.h>

using namespace std;
#define BIZHANG 6
#define MINDIST 0.8

#define div 2

int state; // flag: 0=find obstacle ;1=move around obstacle
int flag = 0;

int cnt = 0;

float findMinDistance(vector<float> ranges) {
  float mindistance = ranges[175];
  for (int i = 220; i < 230; i++) {
    if (ranges[i] < mindistance) {
      mindistance = ranges[i];
    }
  }
  return mindistance;
}

void laserCallback(const sensor_msgs::LaserScan::ConstPtr &msg) {
  ros::param::get("state", state);

  if (findMinDistance(msg->ranges) > MINDIST && state != BIZHANG) {
    return;
  } else if (findMinDistance(msg->ranges) <= MINDIST && state != BIZHANG) {
    int state_last = state;
    ros::param::set("state", BIZHANG);

    cout << "\033[44mfind obstacle\033[0m" << endl;
    ros::param::set("gear", 1);
    for (int i = 0; i < 200 / div; i++) {
      ros::param::set("state", BIZHANG);
      usleep(10000);
    }
    //        ros::param::set("gear", 2);
    //        for ( int i = 0; i < 150 / div; i++ ) {   // 斜300
    //            ros::param::set("state", BIZHANG);
    //            usleep(10000);
    //        }

    ros::param::set("gear", 5);
    for (int i = 0; i < 450 / div; i++) { // 斜700
      ros::param::set("state", BIZHANG);
      usleep(10000);
    }
    ros::param::set("gear", 2);
    for (int i = 0; i < 150 / div; i++) { // 斜75
      ros::param::set("state", BIZHANG);
      usleep(10000);
    }
    cout << "\033[44mavoid over\033[0m" << endl;
    ros::param::set("state", state_last);
  }
  flag = 1;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "avoid");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("/scan", 1, laserCallback);
  ros::Rate looprate(100);
  int light = 0;
  //    while(light == 0)
  //    {
  //        ros::param::get("light", light);
  //        usleep(10000);
  //    }
  //    sleep(10);
  int stim_last = 1;
  int stim_cur = 1;
  while (ros::ok()) {
    ros::param::get("stim", stim_cur);
    if (stim_last == 1 && stim_cur != 1) {
      cnt++;
    }
    if (cnt < 1) {
      looprate.sleep();
      continue;
    }
    else if(cnt == 1){
      sleep(10);
      cnt++;
    }

    if (flag == 0)
      ros::spinOnce();
    else {
      break;
    }
    looprate.sleep();
  }
  return 0;
}
